﻿/// 查找圆的算法
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#include <math.h>
#include <iostream>

using namespace cv;
using namespace std;

Mat g_gray;
int g_param1 = 200;
int g_param2 = 9;
static void on_Params1Change(int value,void*)
{
    cout << "value1:" << value << endl;
    vector<Vec3f> circles;
    HoughCircles(g_gray, circles, HOUGH_GRADIENT, 1,
                 10,  // change this value to detect circles with different distances to each other
                 g_param1, g_param2, 2, 30 // change the last two parameters
            // (min_radius & max_radius) to detect larger circles
    );

    cout << "find " << circles.size() << " circles." << endl;
    Mat drawing = Mat::zeros(g_gray.size(),CV_8UC3);
    for( size_t i = 0; i < circles.size(); i++ )
    {
        cout <<  i << ": " << circles[i] << endl;
        Vec3i c = circles[i];
        Point center = Point(c[0], c[1]);
        // circle center
        // circle( drawing, center, 1, Scalar(0,100,100), 3, LINE_AA);
        // circle outline
        int radius = c[2];
        circle( drawing, center, radius, Scalar(255,0,255), 3, LINE_AA);
    }
    imshow("circles",drawing);
}

static void on_Params2Change(int value,void*)
{
    cout << "value2:" << value << endl;
    vector<Vec3f> circles;
    HoughCircles(g_gray, circles, HOUGH_GRADIENT, 2,
                 10,  // change this value to detect circles with different distances to each other
                 g_param1, g_param2, 2, 30 // change the last two parameters
            // (min_radius & max_radius) to detect larger circles
    );

    cout << "find " << circles.size() << " circles." << endl;
    Mat drawing = Mat::zeros(g_gray.size(),CV_8UC3);
    for( size_t i = 0; i < circles.size(); i++ )
    {
        cout <<  i << ": " << circles[i] << endl;
        Vec3i c = circles[i];
        Point center = Point(c[0], c[1]);
        // circle center
        // circle( drawing, center, 1, Scalar(0,100,100), 3, LINE_AA);
        // circle outline
        int radius = c[2];
        circle( drawing, center, radius, Scalar(255,0,255), 3, LINE_AA);
    }
    imshow("circles",drawing);
}

static int findHoughCirclesWithTrackbar(const char * filename)
{
    Mat src = imread(filename, IMREAD_GRAYSCALE);
    g_gray = src.clone();
    // smooth it, otherwise a lot of false circles may be detected
    // GaussianBlur(gray, gray, Size(9, 9), 2, 2);
    // medianBlur(g_gray, g_gray, 3);

    namedWindow("circles", WINDOW_AUTOSIZE);
    createTrackbar("Param1: ", "circles", &g_param1, 255, on_Params1Change);
    createTrackbar("Param2: ", "circles", &g_param2, 255, on_Params2Change);
    on_Params1Change(g_param1, 0);
    waitKey();
    return 0;
}


static int findHoughCircles(const char * filename)
{
    Mat src = imread(filename, IMREAD_GRAYSCALE);
    Mat gray = src.clone();
    // smooth it, otherwise a lot of false circles may be detected
    // GaussianBlur(gray, gray, Size(9, 9), 2, 2);
    // medianBlur(gray, gray, 3);

    vector<Vec3f> circles;
    HoughCircles(gray, circles, HOUGH_GRADIENT, 1,
                 8,  // change this value to detect circles with different distances to each other
                 220, 9, 1, 30 // change the last two parameters
            // (min_radius & max_radius) to detect larger circles
    );

    cout << "find " << circles.size() << " circles." << endl;
    for( size_t i = 0; i < circles.size(); i++ )
    {
        cout <<  i << ": " << circles[i] << endl;
        Vec3i c = circles[i];
        Point center = Point(c[0], c[1]);
        // circle center
        circle( src, center, 1, Scalar(0,100,100), 3, LINE_AA);
        // circle outline
        int radius = c[2];
        circle( src, center, radius, Scalar(255,0,255), 3, LINE_AA);
    }
    //imshow("src circles", src);
    //imshow("circles", src);
    //waitKey();
    return 0;
}

/// 查找目录下面的多个铺面的圆
static void findHoughCirclesInDir()
{
    string baseFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball5/";
    const char * filePattern = "C0_%d.bmp";
    char buf[30];
    int nImage = 2;
    for (int i = 1; i <= nImage; i++) {
        snprintf(buf, 30, filePattern, i);
        string filename = baseFolder + buf;
        cout << i <<"==========: " <<  filename << endl;
        findHoughCircles(filename.c_str());
        if (false) { // 有移动的图像
            snprintf(buf, 30, filePattern, i+10);
            filename = baseFolder + buf;
            cout << i <<"==========: " <<  filename << endl;
            findHoughCircles(filename.c_str());
        }
    }
}

/// 通过质量中心计算圆心, threshold 一般取80
/// @factor 物体的 min(长，宽)/max(长，宽) 最小比例因子
static int findCircleCenters(const char * filename, double threshold, double factor,
                             double minD, double maxD, vector<Point2f> &centers)
{
    Mat src = imread(filename, IMREAD_GRAYSCALE);
    Mat gray = src;

    //轮廓
    vector<vector<Point>> contours;

    //使用canny检测出边缘
    Mat edge_image;
    double threshold1 = threshold;  // 经验值 80
    Canny(gray, edge_image, threshold1,threshold1*3);

    // 边缘追踪，没有存储边缘的组织结构
    findContours(edge_image, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);


    //计算图像矩, 图像的质心
    vector<Moments> mu(contours.size());
    vector<Point2f> mc(contours.size());
    vector<Rect> boundRect(contours.size());
    for (uint i = 0; i < contours.size(); i++)
    {
        mu[i] = moments(contours[i], false);
        boundRect[i] = boundingRect(Mat(contours[i]));
        mc[i] = Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
    }

    for (uint i = 0; i < mc.size(); i++) {
        if( (float)min(boundRect[i].width, boundRect[i].height) / (float)max(boundRect[i].width, boundRect[i].height) >= factor
                && boundRect[i].width > minD && boundRect[i].height > minD
                && boundRect[i].width < maxD && boundRect[i].height < maxD)
        {
            centers.push_back(mc[i]);
        }
    }
    return 0;
}

/// 测试模糊对图像的影响
static int demoBlur(const char * filename)
{
    Mat src = imread(filename, IMREAD_GRAYSCALE);
    Mat gray = src.clone();
    Mat threshold_output;

    medianBlur(gray, gray, 3); // 3 比较合适
    threshold(gray,threshold_output,80,255,THRESH_BINARY);

    //imshow("src", src);
    imshow("dst", gray);
    imshow("threshold_output", threshold_output);
    waitKey();
    return 0;
}

/// Cannay参数对系统的影响
/// 一般参数是 80, 240 比较合适
static int demoCanny(const char * filename)
{
    Mat src = imread(filename, IMREAD_GRAYSCALE);
    Mat gray = src.clone();

    //使用canny检测出边缘
    Mat edge_image;
    for (int threshold1 = 10; threshold1< 100; threshold1 +=10) {
        Canny(gray, edge_image, threshold1,threshold1*3);
        Mat showImg;
        resize(edge_image, showImg, Size(edge_image.cols / 2, edge_image.rows / 2), (0, 0), (0, 0), 3);
        //imshow("canny1",edge_image);
        imshow("canny1",showImg);
        waitKey();
    }

    return 0;
}

/// 找到轮廓的中心位置
static int demoFindContours(const char * filename)
{
    Mat src = imread(filename, IMREAD_GRAYSCALE);
    Mat gray = src.clone();

    //轮廓
    vector<vector<Point>> contours;

    //使用canny检测出边缘
    Mat edge_image;
    double threshold1 = 80;  // 经验值
    Canny(gray, edge_image, threshold1,threshold1*3);

    // 边缘追踪，没有存储边缘的组织结构
    findContours(edge_image, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);


    //计算图像矩
    vector<Moments> mu(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
        mu[i] = moments(contours[i], false);
    }

    //计算图像的质心
    vector<Point2f> mc(contours.size());
    vector<Rect> boundRect(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
        boundRect[i] = boundingRect(Mat(contours[i]));
        mc[i] = Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
    }

    // 自己计算中心点
    vector<Point2f> center(contours.size());
    vector<float> radius(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
        int count = contours[i].size();

        double x = 0.0, y = 0.0;
        for (int j=0; j < count; j++) {
            x += contours[i][j].x;
            y += contours[i][j].y;
        }
        center[i].x = x/(double)count;
        center[i].y = y/(double)count;
        double rr = 0;
        for (int j=0; j < count; j++) {
            rr += (contours[i][j].x - center[i].x) * (contours[i][j].x - center[i].x)
                    + (contours[i][j].y - center[i].y) * (contours[i][j].y - center[i].y);
        }
        radius[i] = (float)sqrt(rr/count);
    }

    Mat drawing = Mat::zeros(edge_image.size(),CV_8UC3);
    cvtColor(gray, drawing, COLOR_GRAY2BGR);
    for(int i = 0 ; i < contours.size(); i++){
        if ( abs(boundRect[i].width - boundRect[i].height)>2 || boundRect[i].width < 5 || boundRect[i].width < 5) {
            continue;
        }
        Scalar color = Scalar(0, 255, 0);
        Scalar color2 = Scalar(0, 0, 255);
        //drawContours(drawing, contours, i, color, 1, 8);
        circle(drawing, mc[i], 1, color2, -1, 8, 0);
        rectangle(drawing,boundRect[i].tl(),boundRect[i].br(),color,1,8,0);
        //circle(drawing,center[i],(int)radius[i],color,2,8,0);
        cout << i+1 << " rect = " << boundRect[i] << endl;
        //cout << i+1 <<  " circle = " << center[i] << ", " << radius[i] << endl;
        cout << i+1 <<  " mc = " << mc[i] << endl;
        // cout << i+1 << " :" << contours[i] << endl;
    }

    Mat outDrawing;
    resize(drawing, outDrawing, Size(drawing.cols / 2, drawing.rows / 2), (0, 0), (0, 0), 3); //放大
    namedWindow("show", WINDOW_AUTOSIZE);
    //imshow("show",drawing);
    imshow("show",outDrawing);

    waitKey();
    return 0;
}

/// 计算两幅图对应的位置的距离，必须是理想的位置
static int demoComputeAimCircles() {
    // D:\sai\opencv\images\hivision\ball1\dual
    const char *filename1 = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball2/C0_1.bmp";
    const char *filename2 = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball2/C0_11.bmp";

    vector<Point2f> centers[2];
    findCircleCenters(filename1, 80.0, 0.9, 2, 50, centers[0]);
    findCircleCenters(filename2, 80.0, 0.9, 2, 50, centers[1]);

    for (uint i=0; i< 2; i++) {
        for (uint j = 0; j < centers[i].size(); j++) {
            cout << i << ":" << j << " = " << centers[i][j] << endl;
        }
    }

    //  验证一下距离
    for (uint i=0; i< 4; i++) {
        float x = centers[0][i].x - centers[1][i].x;
        float y = centers[0][i].y - centers[1][i].y;
        cout << i << ": " << x << ", " << y << ". dis = " << sqrt(x*x+y*y) << endl;
    }

    return 0;
}

/// 计算两幅图对应的位置的距离，必须是理想的位置
static int demoComputeCircles() {
    // D:\sai\opencv\images\hivision\ball1\dual
    //const char *filename1 = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball2/C0_1.bmp";
    //const char *filename2 = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball2/C0_11.bmp";
    const char *filename1 = "D:/sai/opencv/images/hivision/ball2/dual/left_1.bmp";
    const char *filename2 = "D:/sai/opencv/images/hivision/ball2/dual/left_2.bmp";

    vector<Point2f> centers[2];
    findCircleCenters(filename1, 80.0, 0.9, 5, 50, centers[0]);
    findCircleCenters(filename2, 80.0, 0.9, 5, 50, centers[1]);

    for (uint i=0; i< 2; i++) {
        for (uint j = 0; j < centers[i].size(); j++) {
            cout << i << ":" << j << " = " << centers[i][j] << endl;
        }
    }

    //  验证一下距离
    for (uint i=0; i< min(centers[0].size(), centers[1].size()); i++) {
        float x = centers[0][i].x - centers[1][i].x;
        float y = centers[0][i].y - centers[1][i].y;
        cout << i << ": " << x << ", " << y << ". dis = " << sqrt(x*x+y*y) << endl;
    }

    return 0;
}

int main(int argc, char const *argv[])
{
    int option = 14;
    const char *filename = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball5/C0_2.bmp";
    const char *filename1 = "D:/sai/opencv/images/hivision/ball2/dual/left_1.bmp";
    switch (option) {
    case 1: {findHoughCircles(filename); break;}
    case 2: {findHoughCirclesWithTrackbar(filename); break;}
    case 3: {findHoughCirclesInDir(); break;}

    case 11: {demoBlur(filename); break;}
    case 12: {demoCanny(filename1); break;}
    case 13: {demoFindContours(filename1); break;}
    case 14: {demoComputeCircles(); break;}
    default: break;
    }

    return 0;
}
